#include "cascadedefin.h"
#include <fstream>
#include <math.h>
#include "cascade.h"
#if 1
#include "cv.h"
#include "highgui.h"
using namespace std;
#endif



bool readCascadeFile( char* cascade_filename, CascadeClassifier *cascade )
{
	/*if ( cascade_filename == NULL || cascade == NULL )
	{
		return false;
	}
	ifstream f;
	f.open( cascade_filename );
	f>>cascade->count;
	f.ignore(256,'\n');
	if ( cascade->count < 1 )
	{
		return false;
	}
	cascade->ac = new AdaBoostClassifier[cascade->count];
	int i;
	for ( i = 0; i < cascade->count; i++ )
	{
		f>>cascade->ac[ i ].count;
		f.ignore(256,'\n');
		printf("----->%d\n", cascade->ac[ i ].count );

		cascade->ac[ i ].scs    = new SimpleClassifier[ cascade->ac[ i ].count ];
		cascade->ac[ i ].alphas = new double[ cascade->ac[ i ].count ];

		f>>cascade->ac[ i ].thresh; 
		f.ignore(256,'\n');

		for(int j=0;j<cascade->ac[ i ].count;j++)
		{
			f>>cascade->ac[ i ].alphas[j];
		}
		f.ignore(256,'\n');

		for(int k=0;k<cascade->ac[ i ].count;k++) 
		{
			f>>cascade->ac[ i ].scs[ k ].thresh>>cascade->ac[ i ].scs[ k ].parity>>cascade->ac[ i ].scs[ k ].type;
			f>>cascade->ac[ i ].scs[ k ].x1>>cascade->ac[ i ].scs[ k ].x2>>cascade->ac[ i ].scs[ k ].x3>>cascade->ac[ i ].scs[ k ].x4>>cascade->ac[ i ].scs[ k ].y1>>cascade->ac[ i ].scs[ k ].y2>>cascade->ac[ i ].scs[ k ].y3>>cascade->ac[ i ].scs[ k ].y4;
			f.ignore(256,'\n'); 
		}
		f.ignore(256,'\n');
	}
	f.close();*/
	return true;
}
static void DrawResults(IntImage& image, objPos *results, int Count )
{
	/*int i;
	unsigned int k;
	int x1,x2,y1,y2;

	for(k=0;k<Count;k++)
	{
		y1 = (results[k].top>=0)?results[k].top:0; 
		y1 = (results[k].top<image.height)?results[k].top:(image.height-1);
		y2 = (results[k].bottom>=0)?results[k].bottom:0;
		y2 = (results[k].bottom<image.height)?results[k].bottom:(image.height-1);
		x1 = (results[k].left>=0)?results[k].left:0;
		x1 = (results[k].left<image.width)?results[k].left:(image.width-1);
		x2 = (results[k].right>=0)?results[k].right:0;
		x2 = (results[k].right<image.width)?results[k].right:(image.width-1);	
		for(i=y1;i<=y2;i++) 
		{
			image.data[i][x1] = 255;
			image.data[i][x2] = 255;
		}
		for(i=x1;i<=x2;i++)
		{
			image.data[y1][i] = 255;
			image.data[y2][i] = 255;
		}
	}*/

	IplImage* show = cvCreateImage( cvSize( image.width, image.height ), 8, 1 );
	//printf("show: w%d h%d\n",image.width, image.height );
	for ( int j = 0; j < show->height; j++ )
	{
		uchar *pd = (uchar*)show->imageData + j*show->widthStep;
		for ( int i = 0; i < show->width; i++ )
		{
			pd[ i ] = image.data[ j ][ i ];
		}
	}

	for ( int i = 0; i < Count; i++ )
	{
		cvDrawRect( show, cvPoint( results[ i ].x, results[ i ].y), cvPoint( results[ i ].x + results[ i ].width, results[ i ].y + results[ i ].height ), cvScalar(255),3 );
	}
	
	char save[128];
	static int xxx = 0;
	sprintf_s( save, "E:\\rex\\%d.jpg", xxx++ );
	cvSaveImage( save, show );

	
	cvNamedWindow("show",0);
	cvShowImage("show",show);
	cvWaitKey(0);

	cvReleaseImage( &show );
}

//inline int SizeOfRect(CRect& rect)
//{
//	int width  = rect.right  - rect.left;
//	int height = rect.bottom - rect.top;
//	return width*height;
//}
//
//void PostProcess(vector<CRect>& result,const int combine_min)
//{
//	vector<CRect> res1;
//	vector<CRect> resmax;
//	vector<int> res2;
//	bool yet;
//	CRect rectInter;
//
//	for(unsigned int i=0,size_i=result.size();i<size_i;i++)
//	{
//		yet = false;
//		CRect& result_i = result[i];
//		for(unsigned int j=0,size_r=res1.size();j<size_r;j++)
//		{
//			CRect& resmax_j = resmax[j];
//			if(rectInter.IntersectRect(result_i,resmax_j))
//			{
//				if(
//					SizeOfRect(rectInter)>0.6*SizeOfRect(result_i) 
//					&& SizeOfRect(rectInter)>0.6*SizeOfRect(resmax_j)
//					)
//				{
//					CRect& res1_j = res1[j];
//					resmax_j.UnionRect(resmax_j,result_i);
//					res1_j.bottom += result_i.bottom;
//					res1_j.top += result_i.top;
//					res1_j.left += result_i.left;
//					res1_j.right += result_i.right;
//					res2[j]++;
//					yet = true;
//					break;
//				}
//			}
//		}
//		if(yet==false)
//		{
//			res1.push_back(result_i);
//			resmax.push_back(result_i);
//			res2.push_back(1);
//		}
//	}
//
//	for(unsigned int i=0,size=res1.size();i<size;i++)
//	{
//		const int count = res2[i];
//		CRect& res1_i = res1[i];
//		res1_i.top /= count;
//		res1_i.bottom /= count;
//		res1_i.left /= count;
//		res1_i.right /= count;
//	}
//
//	result.clear();
//	for(unsigned int i=0,size=res1.size();i<size;i++) 
//		if(res2[i]>combine_min)
//			result.push_back(res1[i]);
//}

#define MAXOBJNUM 256
int _nodes[MAXOBJNUM * 2];
static int SimilarRects(const double eps,const objPos r1,const objPos r2)
{
	double delta = eps * 0.5 * (MIN(r1.width,r2.width) + MIN(r1.height,r2.height ));

	if ( abs(r1.x - r2.x) <= delta &&abs(r1.y - r2.y) <= delta &&
		abs(r1.x + r1.width - r2.x - r2.width) <= delta &&
		abs(r1.y + r1.height - r2.y - r2.height) <= delta)
	{
		return 1;
	}
	else
	{
		return 0;
	}

}
static int partition(objPos _vec[],int labels[],const double eps,const int objNum)
{
	int i, j, N =  objNum;
	int nclasses = 0;
	objPos* vec = &_vec[0];

	int PARENT=0;
	int RANK=1;

	//vector<int> _nodes(N*2);
	int (*nodes)[2] = (int(*)[2])&_nodes[0];

	// The first O(N) pass: create N single-vertex trees
	for(i = 0; i < N; i++)
	{
		nodes[i][PARENT]=-1;
		nodes[i][RANK] = 0;
	}

	// The main O(N^2) pass: merge connected components
	for( i = 0; i < N; i++ )
	{
		int root = i;

		// find root
		while( nodes[root][PARENT] >= 0 )
			root = nodes[root][PARENT];

		for( j = i+1; j < N; j++ )
		{
			int root2;
			if( i == j || !SimilarRects(eps,vec[i], vec[j]))
				continue;

			root2 = j;

			while( nodes[root2][PARENT] >= 0 )
				root2 = nodes[root2][PARENT];

			if( root2 != root )
			{
				int k,parent;
				// unite both trees
				int rank = nodes[root][RANK], rank2 = nodes[root2][RANK];
				if( rank > rank2 )
					nodes[root2][PARENT] = root;
				else
				{
					nodes[root][PARENT] = root2;
					nodes[root2][RANK] += rank == rank2;
					root = root2;
				}
				assert( nodes[root][PARENT] < 0 );

				k = j;
				// compress the path from node2 to root
				while( (parent = nodes[k][PARENT]) >= 0 )
				{
					nodes[k][PARENT] = root;
					k = parent;
				}

				// compress the path from node to root
				k = i;
				while( (parent = nodes[k][PARENT]) >= 0 )
				{
					nodes[k][PARENT] = root;
					k = parent;
				}
			}
		}
	}

	// Final O(N) pass: enumerate classes
	//labels.resize(N);

	for( i = 0; i < N; i++ )
	{
		int root = i;
		while( nodes[root][PARENT] >= 0 )
			root = nodes[root][PARENT];
		// re-use the rank as the class label
		if( nodes[root][RANK] >= 0 )
			nodes[root][RANK] = ~nclasses++;
		labels[i] = ~nodes[root][RANK];
	}

	return nclasses;
}
static int  groupRectangles(objPos rectList[], int labels[], objPos rrects[], int rweights[], const int groupThreshold, const double eps,const int objNum)
{
	int nclasses = 0;

	int i, j, nlabels = objNum;
	int iGroupCount = 0;

	memset(rrects,0,sizeof(objPos) *MAXOBJNUM);
	memset(rweights,0,sizeof(int) *MAXOBJNUM);
	memset(labels,0,sizeof(int) * MAXOBJNUM);

	if( groupThreshold <= 0 )
	{
		return 0;
	}

	nclasses = partition(rectList, labels, eps, objNum);

	// count number of neighbors
	for( i = 0; i < nlabels; i++ )
	{
		int cls = labels[i];
		rrects[cls].x += rectList[i].x;
		rrects[cls].y += rectList[i].y;
		rrects[cls].width += rectList[i].width;
		rrects[cls].height += rectList[i].height;
		rweights[cls]++;
	}

	// calculate average bounding box
	for( i = 0; i < nclasses; i++ )
	{
		objPos r = rrects[i];
		float s = 1.f/rweights[i];
		rrects[i] = objpos(cvRound(r.x * s),cvRound(r.y*s),cvRound(r.width * s),cvRound(r.height * s));
	}

	memset(rectList,0,sizeof(objPos)*MAXOBJNUM);

	// filter out small face rectangles inside large rectangles
	for( i = 0; i < nclasses; i++ )
	{
		objPos r1 = rrects[i];
		int n1 = rweights[i];
		if( n1 < groupThreshold )
			continue;

		for( j = 0; j < nclasses; j++ )
		{
			int n2 = rweights[j];
			objPos r2 = rrects[j];
			int dx = cvRound(r2.width * eps );
			int dy = cvRound( r2.height * eps );

			if( j == i || n2 < groupThreshold )
				continue;

			if( i != j &&
				r1.x >= r2.x - dx &&
				r1.y >= r2.y - dy &&
				r1.x + r1.width <= r2.x + r2.width + dx &&
				r1.y + r1.height <= r2.y + r2.height + dy &&
				(n2 > MAX(3, n1) || n1 < 3) )
				break;
		}

		if( j == nclasses )
		{
			rectList[iGroupCount++] = r1;
		}
	}

	return iGroupCount;

}

int detectface( IntImage& original, objPos res[256], int minsize, int maxsize, float scale )
{
	IntImage procface;
	IntImage image,square;
	double sq,ex,value;
	int result;
	double ratio;
	objPos results[512];
	const AdaBoostClassifier* ac = g_cascade.ac;
	int sx = 18;
	int sy = 18;
	int counts = 0;
	double mean_min = 5410.8,mean_max = 84911.2,var_min = 7.52304,var_max = 110.513,sq_min = 112811;
	int sbg = 0;
	int labels[MAXOBJNUM];
	objPos rrects[MAXOBJNUM];
	int rweights[MAXOBJNUM];
	int objss = 0;

	memset( results, 0, sizeof(objPos)*512 );

	procface.Copy(original);
	ratio = 1.0;
	double paddedsize = double(1)/double((sx+1)*(sy+1));
	while((procface.height>sx+1) && (procface.width>sy+1))
	{
		procface.CalcSquareAndIntegral(square,image);
		for(int i=0,size_x=image.height-sx;i<size_x;i+=2)
			for(int j=0,size_y=image.width-sy;j<size_y;j+=2)
			{
				ex = image.data[i+sx][j+sy]+image.data[i][j]-image.data[i+sx][j]-image.data[i][j+sy];
				if(ex<mean_min || ex>mean_max) continue;
				sq = square.data[i+sx][j+sy]+square.data[i][j]-square.data[i+sx][j]-square.data[i][j+sy];
				if(sq<sq_min) continue;
				ex *= paddedsize;
				ex = ex * ex;
				sq *= paddedsize;
				sq = sq - ex;
				if(sq>0) sq = sqrt(sq); else sq = 1.0;
				if(sq<var_min) continue;
				result = 1;
				sbg = 0;
				for(int k=0; k < g_cascade.count;k++)
				{
					value = 0.0;
					sbg++;
					for(int t=0,size=ac[k].count;t<size;t++)
					{
						REAL f1 = 0;
						REAL** p = image.data + i;
						const SimpleClassifier& s = ac[k].scs[t];

						switch(s.type)
						{
						case 0:
							f1 =   p[s.x1][j+s.y3] - p[s.x1][j+s.y1] + p[s.x3][j+s.y3] - p[s.x3][j+s.y1]
							+ (2)*(p[s.x2][j+s.y1] - p[s.x2][j+s.y3]);
							break;
						case 1:
							f1 =   p[s.x3][j+s.y1] + p[s.x3][j+s.y3] - p[s.x1][j+s.y1] - p[s.x1][j+s.y3]
							+ (2)*(p[s.x1][j+s.y2] - p[s.x3][j+s.y2]);
							break;
						case 2:
							f1 =   p[s.x1][j+s.y1] - p[s.x1][j+s.y3] + p[s.x4][j+s.y3] - p[s.x4][j+s.y1]
							+ (3)*(p[s.x2][j+s.y3] - p[s.x2][j+s.y1] + p[s.x3][j+s.y1]  - p[s.x3][j+s.y3]);
							break;
						case 3:
							f1 =   p[s.x1][j+s.y1] - p[s.x1][j+s.y4] + p[s.x3][j+s.y4] - p[s.x3][j+s.y1]
							+ (3)*(p[s.x3][j+s.y2] - p[s.x3][j+s.y3] + p[s.x1][j+s.y3] - p[s.x1][j+s.y2]);
							break;
						case 4:
							f1 =   p[s.x1][j+s.y1] + p[s.x1][j+s.y3] + p[s.x3][j+s.y1] + p[s.x3][j+s.y3]
							- (2)*(p[s.x2][j+s.y1] + p[s.x2][j+s.y3] + p[s.x1][j+s.y2] + p[s.x3][j+s.y2])
								+ (4)*p[s.x2][j+s.y2];
							break;
						default:
#ifndef DEBUG 
							__assume(0);
#else
							;
#endif
						}
						if(s.parity!=0)
							if(f1<sq*s.thresh)
								value += ac[k].alphas[t];
							else ;
						else
							if(f1>=sq*s.thresh)
								value += ac[k].alphas[t];
							else ;
					}
					if(value<ac[k].thresh) 
					{
						result = 0;
						break;
					}
				}
				if(result!=0) 
				{
					const float r = 1.0/ratio;
					results[ counts ].x = (int)(j*r);
					results[ counts ].y = (int)(i*r);
					results[ counts ].width = sy*r;
					results[ counts ].height = sx*r;
					counts++;
					if ( counts >= 256 )
					{
						break;
					}
				}
				if ( sbg < 3 )
				{
					j += 2;
				}
				
			}

			ratio = ratio*scale;
			procface.Resize(image,scale);
			//printf("%d %d\n", procface.width, procface.height );
			SwapIntImage(procface,image);
	}

	
	objss = groupRectangles( results, labels, rrects, rweights, 1, 0.4, counts );
	if ( objss >= 50 )
	{
		objss = 50;
	}
	memcpy( res, results, sizeof(objPos)*objss );
	//DrawResults(original, results, objss);
	//printf("all faces: %d\n", objss );
	//printf("really: %d\n", objss );
	return objss;
}
static float GetMaskRate(int *interl, int step, int x, int y, int width, int height )
{
	int iWidth  = width;
	int iHeight = height;
	int iX = x + iWidth;
	int iY = y + iHeight;

	int iPixNum = iWidth*iHeight;
	int dSum = *( interl + y*step + x ) + *( interl + iY*step + iX );
	//printf("%d\n",dSum);
	//int dSum = mask.at<int>(cv::Point(Position.x, Position.y))+mask.at<int>(cv::Point(iX, iY));

	//dSum -= mask.at<int>(cv::Point(Position.x, iY));
	dSum -= *( interl + iY*step + x);

	//dSum -= mask.at<int>(cv::Point(iX, Position.y));
	dSum -= *( interl + y*step + iX);

	return (float)dSum/(float)(iPixNum);
}
int detectface_mask( IntImage& original, objPos res[256], int minsize, int maxsize, float scale, int *maskinter )
{
	IntImage procface;
	IntImage image,square;
	double sq,ex,value;
	int result;
	double ratio;
	objPos results[512];
	const AdaBoostClassifier* ac = g_cascade.ac;
	int sx = 18;
	int sy = 18;
	int counts = 0;
	double mean_min = 5410.8,mean_max = 84911.2,var_min = 7.52304,var_max = 110.513,sq_min = 112811;
	int sbg = 0;
	int labels[MAXOBJNUM];
	objPos rrects[MAXOBJNUM];
	int rweights[MAXOBJNUM];
	int objss = 0;
	int current_w = sx, current_h = sy;

	memset( results, 0, sizeof(objPos)*512 );

	procface.Copy(original);
	ratio = 1.0;
	double paddedsize = double(1)/double((sx+1)*(sy+1));
	while((procface.height>sx+1) && (procface.width>sy+1))
	{
		if ( current_w < minsize )
		{
			procface.Resize(image,scale);
			SwapIntImage(procface,image);
			ratio = ratio*scale;
			current_w = current_w/ratio;
			continue;
		}
		if ( current_w > maxsize )
		{
			break;
		}
		procface.CalcSquareAndIntegral(square,image);
		for(int i=0,size_x=image.height-sx;i<size_x;i+=2)
			for(int j=0,size_y=image.width-sy;j<size_y;j+=2)
			{
				if ( GetMaskRate( maskinter, original.width + 1, j/ratio, i/ratio, sx/ratio, sy/ratio ) < 0.4 )
				{
					continue;
				}
				ex = image.data[i+sx][j+sy]+image.data[i][j]-image.data[i+sx][j]-image.data[i][j+sy];
				if(ex<mean_min || ex>mean_max) continue;
				sq = square.data[i+sx][j+sy]+square.data[i][j]-square.data[i+sx][j]-square.data[i][j+sy];
				if(sq<sq_min) continue;
				ex *= paddedsize;
				ex = ex * ex;
				sq *= paddedsize;
				sq = sq - ex;
				if(sq>0) sq = sqrt(sq); else sq = 1.0;
				if(sq<var_min) continue;
				result = 1;
				sbg = 0;
				for(int k=0; k < g_cascade.count;k++)
				{
					value = 0.0;
					sbg++;
					for(int t=0,size=ac[k].count;t<size;t++)
					{
						REAL f1 = 0;
						REAL** p = image.data + i;
						const SimpleClassifier& s = ac[k].scs[t];

						switch(s.type)
						{
						case 0:
							f1 =   p[s.x1][j+s.y3] - p[s.x1][j+s.y1] + p[s.x3][j+s.y3] - p[s.x3][j+s.y1]
							+ (2)*(p[s.x2][j+s.y1] - p[s.x2][j+s.y3]);
							break;
						case 1:
							f1 =   p[s.x3][j+s.y1] + p[s.x3][j+s.y3] - p[s.x1][j+s.y1] - p[s.x1][j+s.y3]
							+ (2)*(p[s.x1][j+s.y2] - p[s.x3][j+s.y2]);
							break;
						case 2:
							f1 =   p[s.x1][j+s.y1] - p[s.x1][j+s.y3] + p[s.x4][j+s.y3] - p[s.x4][j+s.y1]
							+ (3)*(p[s.x2][j+s.y3] - p[s.x2][j+s.y1] + p[s.x3][j+s.y1]  - p[s.x3][j+s.y3]);
							break;
						case 3:
							f1 =   p[s.x1][j+s.y1] - p[s.x1][j+s.y4] + p[s.x3][j+s.y4] - p[s.x3][j+s.y1]
							+ (3)*(p[s.x3][j+s.y2] - p[s.x3][j+s.y3] + p[s.x1][j+s.y3] - p[s.x1][j+s.y2]);
							break;
						case 4:
							f1 =   p[s.x1][j+s.y1] + p[s.x1][j+s.y3] + p[s.x3][j+s.y1] + p[s.x3][j+s.y3]
							- (2)*(p[s.x2][j+s.y1] + p[s.x2][j+s.y3] + p[s.x1][j+s.y2] + p[s.x3][j+s.y2])
								+ (4)*p[s.x2][j+s.y2];
							break;
						default:
#ifndef DEBUG 
							__assume(0);
#else
							;
#endif
						}
						if(s.parity!=0)
							if(f1<sq*s.thresh)
								value += ac[k].alphas[t];
							else ;
						else
							if(f1>=sq*s.thresh)
								value += ac[k].alphas[t];
							else ;
					}
					if(value<ac[k].thresh) 
					{
						result = 0;
						break;
					}
				}
				if(result!=0) 
				{
					const float r = 1.0/ratio;
					results[ counts ].x = (int)(j*r);
					results[ counts ].y = (int)(i*r);
					results[ counts ].width = sy*r;
					results[ counts ].height = sx*r;
					counts++;
					if ( counts >= 256 )
					{
						break;
					}
				}
				if ( sbg < 3 )
				{
					j += 2;
				}
			}
			ratio = ratio*scale;
			procface.Resize(image,scale);
			SwapIntImage(procface,image);
			current_w = current_w/ratio;
	}
	objss = groupRectangles( results, labels, rrects, rweights, 1, 0.4, counts );
	if ( objss >= 50 )
	{
		objss = 50;
	}
	memcpy( res, results, sizeof(objPos)*objss );
	return objss;
}
int  kise_detectface( unsigned char* grayimg, int width, int height, int widthstep, objPos res[256], int minsize, int maxsize, float scale, int* maskinter )
{
	static IntImage img;
	img.create( grayimg, width, height, widthstep);
	return detectface_mask( img, res, minsize, maxsize, scale, maskinter );
	return detectface( img, res, minsize, maxsize, scale );
}